#include "PubMath.h"

#include "Bldc_Svc.h"
#include "Bldc_Svc_private.h"

void Hall_Signal_Cnt_05T_Init(uint16_T *rty_o_y0, uint16_T *rty_o_y1)
{
  *rty_o_y0 = 0U;
  *rty_o_y1 = 0U;
}


void Hall_Signal_Cnt_05T(uint8_T rtu_i_x, uint16_T rtu_i_Max_05HoldTime,
  uint16_T *rty_o_y0, uint16_T *rty_o_y1, DW_Hall_Signal_Cnt_05T_T *localDW)
{
  switch (rtu_i_x) {
   case 1:
    if (localDW->cnt1 >= rtu_i_Max_05HoldTime) {
      localDW->cnt1 = rtu_i_Max_05HoldTime;
      localDW->y1 = localDW->cnt1;
      localDW->y0 = 0U;
    } else {
      localDW->cnt1++;
      localDW->y0 = localDW->pre_y0;
    }

    localDW->pre_y1 = localDW->cnt1;
    localDW->cnt0 = 0U;
    break;

   case 0:
    if (localDW->cnt0 >= rtu_i_Max_05HoldTime) {
      localDW->cnt0 = rtu_i_Max_05HoldTime;
      localDW->y0 = localDW->cnt0;
      localDW->y1 = 0U;
    } else {
      localDW->cnt0++;
      localDW->y1 = localDW->pre_y1;
    }

    localDW->pre_y0 = localDW->cnt0;
    localDW->cnt1 = 0U;
    break;
  }

  *rty_o_y0 = localDW->y0;
  *rty_o_y1 = localDW->y1;
}

void Hall_Signal_Cnt_T(void)
{
  if (Bldc_Svc_B.o_Hall_Signal[0] > Bldc_Svc_DW.pre_x) {
    Bldc_Svc_DW.cnt = 1U;
  } else {
    Bldc_Svc_DW.cnt++;
  }

  Bldc_Svc_DW.pre_x = Bldc_Svc_B.o_Hall_Signal[0];
}

void N32RC1_LPF_MBD_Init(DW_N32RC1_LPF_MBD_T *localDW, P_N32RC1_LPF_MBD_T
  *localP)
{
  localDW->UnitDelay_DSTATE = localP->UnitDelay_InitialCondition;
}

void N32RC1_LPF_MBD(int16_T rtu_Inp, uint16_T rtu_kx, uint16_T rtu_ky, int32_T
                    *rty_Fil, DW_N32RC1_LPF_MBD_T *localDW)
{
  *rty_Fil = N32ShiftRight(rtu_Inp * rtu_kx + localDW->UnitDelay_DSTATE * rtu_ky, C_NUM15);
}

void N32RC1_LPF_MBD_Update(int32_T *rty_Fil, DW_N32RC1_LPF_MBD_T *localDW)
{
  localDW->UnitDelay_DSTATE = *rty_Fil;
}

void N32Step_MBD_Init(int32_T *rty_o_Out)
{
  *rty_o_Out = 0;
}

void N32Step_MBD(int16_T rtu_Now, int16_T rtu_Step, uint8_T rtu_Reset, int16_T
                 rtu_y0, int32_T *rty_o_Out, DW_N32Step_MBD_T *localDW)
{
  int32_T StepErr;
  StepErr = rtu_Now - localDW->Pre;
  if (StepErr > rtu_Step) {
    StepErr = localDW->Pre + rtu_Step;
  } else if (StepErr < -rtu_Step) {
    StepErr = localDW->Pre - rtu_Step;
  } else {
    StepErr = rtu_Now;
  }

  if (rtu_Reset == 1) {
    localDW->Pre = rtu_y0;
  } else {
    localDW->Pre = StepErr;
  }

  *rty_o_Out = StepErr;
}

void Position_PID_Qn_Init(DW_Position_PID_Qn_T *localDW, P_Position_PID_Qn_T
  *localP)
{
  localDW->UnitDelay_DSTATE = localP->UnitDelay_InitialCondition;
  localDW->UnitDelay1_DSTATE = localP->UnitDelay1_InitialCondition;
}

void Position_PID_Qn(int32_T rtu_Ref, int16_T rtu_Fdb, int32_T rtu_Kp, int32_T
                     rtu_KiTs, int32_T rtu_Kc, uint8_T rtu_Kp_Qn, uint8_T
                     rtu_Ki_Qn, uint8_T rtu_Reset, int32_T rtu_y0, int32_T
                     *rty_Out, DW_Position_PID_Qn_T *localDW,
                     P_Position_PID_Qn_T *localP, uint8_T rtp_Out_Qn, uint8_T
                     rtp_Kc_Qn, int32_T rtp_Upper_Limit, int32_T rtp_Lower_Limit)
{
  int32_T rtb_FunctionCaller2;
  int32_T rtb_Saturation;
  int32_T rtb_UnitDelay_f;
  int32_T rtu_Reset_0;
  rtb_Saturation = rtu_Ref - rtu_Fdb;
  rtb_FunctionCaller2 = N32ShiftRight(rtu_Kp * rtb_Saturation, rtu_Kp_Qn);
  rtb_UnitDelay_f = localDW->UnitDelay_DSTATE;
  rtb_Saturation = N32ShiftRight(rtb_Saturation * rtu_KiTs, rtu_Ki_Qn);
  if (rtu_Reset > localP->Switch_Threshold) {
    rtb_UnitDelay_f = rtu_y0;
  } else {
    if (rtu_Reset > localP->Switch2_Threshold) {
      rtu_Reset_0 = localP->Constant_Value;
    } else {
      rtu_Reset_0 = localDW->UnitDelay1_DSTATE;
    }

    rtb_UnitDelay_f = (rtb_UnitDelay_f + rtb_Saturation) + rtu_Reset_0;
  }

  if (rtb_UnitDelay_f > rtp_Upper_Limit) {
    localDW->UnitDelay_DSTATE = rtp_Upper_Limit;
  } else if (rtb_UnitDelay_f < rtp_Lower_Limit) {
    localDW->UnitDelay_DSTATE = rtp_Lower_Limit;
  } else {
    localDW->UnitDelay_DSTATE = rtb_UnitDelay_f;
  }

  if (rtu_Reset > localP->Switch1_Threshold) {
    rtb_UnitDelay_f = rtu_y0;
  } else {
    rtb_UnitDelay_f = rtb_FunctionCaller2 + localDW->UnitDelay_DSTATE;
  }

  if (rtb_UnitDelay_f > rtp_Upper_Limit) {
    rtb_Saturation = rtp_Upper_Limit;
  } else if (rtb_UnitDelay_f < rtp_Lower_Limit) {
    rtb_Saturation = rtp_Lower_Limit;
  } else {
    rtb_Saturation = rtb_UnitDelay_f;
  }

  *rty_Out = N32ShiftRight(rtb_Saturation, rtp_Out_Qn);
  localDW->UnitDelay1_DSTATE = N32ShiftRight((rtb_Saturation - rtb_UnitDelay_f) *
    rtu_Kc, rtp_Kc_Qn);
}

void Bldc_Svc_FuncQn_Init(uint8_T *rty_o_y)
{
  *rty_o_y = 0U;
}

void Bldc_Svc_FuncQn(real32_T rtu_i_x, uint8_T rtu_i_sys_Qn_max, uint8_T
                     rtu_i_Qn, uint8_T *rty_o_y, DW_FuncQn_Bldc_Svc_T *localDW)
{
  real32_T x;
  uint8_T i;
  boolean_T exitg1;
  if (rtu_i_x < 32768.0F) {
    x = rtu_i_x;
  } else {
    x = 32767.0F;
  }

  if (x >= 1.0F) {
    i = 0U;
    exitg1 = false;
    while ((!exitg1) && (i < 16)) {
      if ((real32_T)pow(2.0, i) > x) {
        localDW->y = i;
        exitg1 = true;
      } else {
        i++;
      }
    }

    *rty_o_y = (uint8_T)((rtu_i_sys_Qn_max - rtu_i_Qn) - localDW->y);
  } else {
    i = 0U;
    exitg1 = false;
    while ((!exitg1) && (i <= 16)) {
      if ((real32_T)pow(2.0, i) >= x * 65536.0F) {
        localDW->y = (uint8_T)(32 - i);
        exitg1 = true;
      } else {
        i++;
      }
    }

    *rty_o_y = localDW->y;
  }
}
